#pragma once

#ifndef __GETMATCH_H_
#define __GETMATCH_H_

#include <opencv2/opencv.hpp>
#include <vector>
#include "Feature.hpp"
using namespace cv;
using namespace std;
using markerAR::Feature;
enum matchType
{
	distCheck = 0x01,
	ratioCheck = 0x02,
	dist_ratioCheck = 0x04,
};

namespace GetMatch
{
    void GetDMatch(const vector<vector<DMatch>> &dmatches, const vector<int>&idx, vector<DMatch> &dmatch);
	 void GetDMatch(vector<DMatch> &dmatch, const vector<int>&idx);
	template<typename T>
	 void GetMaskObj(vector<T> &objArray, const Mat& mask);
	template<typename T>  
	void GetMaskObj(vector<T> &objArray, const vector<int>&mask);
    template<typename T>
     void GetUnMaskObj(const vector<T> &objArray,const vector<int>&unmask,vector<T> &dst);
	 void GetInliers(const vector<vector<DMatch>> &dmatches, vector<int> &idx, double _ratio, double _distance = -1);
	 void GetInliers (const vector<DMatch> &dmatch, vector<int> &idx,double _distance=90);
	 void GetInliersRadius(const vector<KeyPoint> &query, const vector<KeyPoint> &train, const vector<vector<DMatch>> &dmatches, vector<int>& idx, double ratio, double distance, double squareDistance);
	 void preparePoint2D(const vector<KeyPoint> &query, const vector<KeyPoint> train, const vector<DMatch> &dmatch, vector<Point2f> &query2D, vector<Point2f> &train2D);
	 void prepare3D_2D(const vector<Point3f> &trainP3D, const vector<KeyPoint> &queryKpts, const vector<DMatch> &dmatch,vector<Point3f> &train3d,vector<Point2f> &query2d);
    void setXYZ(const vector<Feature*>&query,const vector<Feature*>&train,const vector<DMatch>&dmatches);
    void preparePoint2D(const vector<markerAR::Feature*> &query, const vector<markerAR::Feature*> train, const vector<DMatch> &dmatch, vector<Point2f> &query2D, vector<Point2f> &train2D);
    void preparePoint2D(const vector<markerAR::Feature*> &query, const vector<markerAR::Feature*> train, const vector<DMatch> &dmatch, vector<Eigen::Vector2d> &query2D, vector<Eigen::Vector2d> &train2D);
};


template<typename T>
void GetMatch::GetMaskObj(vector<T> &objArray, const vector<int>&mask)
{
	vector<T> temp;
	for (const auto &i : mask)
	{
		temp.push_back(objArray[i]);
	}
	objArray = temp;
}
template<typename T>
void GetMatch::GetMaskObj(vector<T> &objArray, const Mat& mask)
{
	CV_Assert(objArray.size() == mask.rows);
	vector<T> temp;
	for (size_t i = 0; i < mask.rows; i++)
	{
		if (mask.at<char>(i))
		{
			temp.push_back(objArray[i]);
		}
	}
	objArray = temp;
}

template<typename T>
void GetMatch::GetUnMaskObj(const vector<T> &objArray,const vector<int>&unmask,vector<T> &dst)
{
    dst.clear();
    dst.reserve(objArray.size()-unmask.size());
    
    for(int i=0,j=0;i<objArray.size();++i)
    {
        if(j == unmask.size()||i!=unmask[j])
            dst.push_back(objArray[i]);
        else
        {
            ++j;
        }
    }
}


#endif